/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "phy_Mujoco.h"

#include "物体/物体.h"
#include "面/面.h"

#include <matXX.h>

#pragma comment(lib,"D:/U/DEV/mujoco/lib/mujoco.lib")




S_PhysicsMujoco::S_PhysicsMujoco() {
	m_模型 = nullptr;
	m_模拟数据 = nullptr;
}

S_PhysicsMujoco::~S_PhysicsMujoco() {
	if (m_模拟数据) {
		mj_deleteData(m_模拟数据);
	}
	if (m_模型) {
		mj_deleteModel(m_模型);
	}
}

void S_PhysicsMujoco::f_Update() {


}










S_物理体* f_PhyMujoco_场景物理体() {

	return nullptr;
}

S_Physics* f_PhysMujoco_创建物理引擎() {
	S_Physics* phy = new S_PhysicsMujoco;
	return phy;
}

void f_PhysMujoco_重置引擎(S_Physics* phy, const std::string& path) {
	S_PhysicsMujoco* mj = static_cast<S_PhysicsMujoco*>(phy);

	/*FILE* f = fopen(path.c_str(), "wt");
	if (f) {
		fwrite(phy->m_XML.c_str(), sizeof(char), phy->m_XML.length(), f);
		fclose(f);
	}*/

	if (mj->m_模型) {
		mj_freeLastXML();
	}

	char error[1000];
	mj->m_模型 = mj_loadXML(path.c_str(), nullptr, error, 1000);
	if (mj->m_模型) {
		mj->m_模拟数据 = mj_makeData(mj->m_模型);
		//mjvScene scn;
		//mjv_makeScene(mj->m_模型, &scn, 2000);
	}
	else {
		mj->m_模拟数据 = nullptr;
	}

	if (mj->m_模拟数据) {
		uint32 body_num = mj->m_Body.size();

		for (uint32 i = 0; i < mj->m_模型->nbody; ++i) {
			for (uint32 j = 0; j < mj->m_模型->body_geomnum[i]; ++j) {
				int32 几何体位置偏移 = mj->m_模型->body_geomadr[i];

				S_物理体* body;
				if (body_num <= 几何体位置偏移) {
					body = new S_物理体();
					//body->m_绑定物体 = f_ob_创建物体(E_物体类型);
					mj->m_Body.push_back(body);
				}
				else {
					body = mj->m_Body[几何体位置偏移];
				}

				S_物理几何体* geom = (S_物理几何体*)body->m_Data;
				if (!body->m_Data) {
					geom = new S_物理几何体();
					body->m_Data = geom;
				}

				std::string name = &mj->m_模型->names[mj->m_模型->name_bodyadr[i]];
				body->m_Name = f_StringToWString(name);


				body->m_物理引擎枚举 = E_物理引擎::E_物理引擎_Mujoco;
				body->m_Type = E_物理体类型::E_物理体_刚体;
				body->m_ID = 几何体位置偏移;
				//body->m_Name;
				//const char* name = mj_id2name(mj->m_模型, 0, 几何体位置偏移);
				//name = mj_id2name(mj->m_模型, 1, i);
				//name = mj_id2name(mj->m_模型, 2, i);
				//name = mj_id2name(mj->m_模型, 3, i);
				//name = mj_id2name(mj->m_模型, mjtObj::mjOBJ_BODY, i);
				//body->m_Name = mj_id2name(mj->m_模型, mj->m_模型->geom_type[几何体位置偏移], 几何体位置偏移);
				//body->m_Name = mj_id2name(mj->m_模型, mj->m_模型->geom_type[几何体位置偏移], i);
				//int32 id = mj_name2id(mj->m_模型, mjtObj::mjOBJ_BODY, body->m_Name.c_str());
				//body->m_ID = id;

				switch (mj->m_模型->geom_type[几何体位置偏移]) {
				case mjtGeom::mjGEOM_PLANE:
					geom->m_Type = E_物理碰撞几何类型::E_碰撞几何_平面;
					break;
				case mjtGeom::mjGEOM_SPHERE:
					geom->m_Type = E_物理碰撞几何类型::E_碰撞几何_球体;
					break;
				case mjtGeom::mjGEOM_CAPSULE:break;
				case mjtGeom::mjGEOM_ELLIPSOID:break;
				case mjtGeom::mjGEOM_CYLINDER:break;
				case mjtGeom::mjGEOM_BOX:
					geom->m_Type = E_物理碰撞几何类型::E_碰撞几何_方体;
					break;
				case mjtGeom::mjGEOM_MESH: {
					//uint32 offset = mj->m_模型->mesh_vertadr[几何体位置偏移];
					//uint32 vertNum = mj->m_模型->mesh_vertnum[几何体位置偏移];

					geom->m_Type = E_物理碰撞几何类型::E_碰撞几何_网格;
					
					break;
				}
				}
			}
		}

		
		for (uint32 i = mj->m_模型->ngeom; i < body_num; ++i) {
			delete mj->m_Body.back();
			mj->m_Body.pop_back();
		}
		//mj->m_模型->ngeom
	}

	return;

	/*auto body_ptr = mj->m_Body.data();
	phy->m_XML = "<mujoco> \n";
	{
		{
			phy->m_XML += "<compiler coordinate = \"local\"  eulerseq = \"xyz\" /> \n";
		}
		phy->m_XML += "	<worldbody> \n";
		for (uint32 i = 0; i < body_num; ++i) {
			auto& body = body_ptr[i];
			phy->m_XML += body->m_XML;
		}
		phy->m_XML += "	</worldbody> \n";
	}
	phy->m_XML += "</mujoco> \n";*/
	//for (uint32 i = 0; i < body_num; ++i) {
	//	body_ptr[i]->m_ID = i;
	//	int32 id = mj_name2id(mj->m_模型, mjOBJ_BODY, "");
	//}
	//
}

void f_PhysMujoco_销毁物理引擎(S_Physics* phy) {
	delete phy;
}

void f_PhyMujoco_解算(S_PhysicsMujoco* mj) {
	if (!mj->m_模拟数据) return;

	mj->m_步进ID;
	//int bodyid = mj_name2id(mj->m_模型, mjOBJ_BODY, "myfloatingbody");
	//mj->m_模拟数据->time = 1 / 60;
	//mj->m_模型->opt.gravity[0] = 0;
	//mj->m_模型->opt.gravity[1] = -9.81;
	//mj->m_模型->opt.gravity[2] = 0;

	mj_step(mj->m_模型, mj->m_模拟数据);

	mj->m_模拟数据->qpos[1];
	mj->m_模拟数据->xpos[1];

	//mat3X3 mat = *((mj->m_模拟数据->geom_xmat);
	uint32 body_num = mj->m_Body.size();
	auto body_ptr = mj->m_Body.data();

	for (uint32 i = 0; i < mj->m_模型->ngeom; ++i) {
		switch (mj->m_模型->geom_type[i]) {
		case mjtGeom::mjGEOM_PLANE:break;
		case mjtGeom::mjGEOM_SPHERE:break;
		case mjtGeom::mjGEOM_CAPSULE:break;
		case mjtGeom::mjGEOM_ELLIPSOID:break;
		case mjtGeom::mjGEOM_CYLINDER:break;
		case mjtGeom::mjGEOM_BOX:break;
		case mjtGeom::mjGEOM_MESH: {
			break;
		}
		}
	}

	/*for (uint32 i = 0; i < mj->m_模型->nbody; ++i) {
		int32 几何体ID = mj->m_模型->body_geomadr[i];
		if (几何体ID >= 0) {
			if (!(body_ptr[几何体ID]->m_绑定物体)) continue;

			mat3X3 rot;
			vec3 pos;
			for (uint8 i = 0; i < 9; ++i) {
				((float32*)&rot)[i] = mj->m_模拟数据->geom_xmat[i + 几何体ID * 9];
			}
			for (uint8 i = 0; i < 3; ++i) {
				((float32*)&pos)[i] = mj->m_模拟数据->geom_xpos[i + 几何体ID * 3];
			}
			f_mat4_构建缩放矩阵(body_ptr[几何体ID]->m_变换.scale);

			S_物体* ob = (S_物体*)body_ptr[几何体ID]->m_绑定物体;
			//Mat44f mat = f_mat44_构建(rot, pos) * f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
			Mat44f mat = f_mat44_构建(rot, pos);
			f_ob_set矩阵(ob, mat);
		}
	}*/

	//mj->m_模型->sensor_objid;

	for (uint32 ob_id = 0; ob_id < body_num; ++ob_id) {
		uint32 几何体ID = body_ptr[ob_id]->m_ID;

		if (!(body_ptr[几何体ID]->m_绑定物体)) continue;
		//int32 id = mj_name2id(mj->m_模型, mjOBJ_BODY, body_ptr[几何体ID]->m_Name.c_str());
		//几何体ID = id;

		mat3X3 rot;
		vec3 pos;
		for (uint8 i = 0; i < 9; ++i) {
			((float32*)&rot)[i] = mj->m_模拟数据->geom_xmat[i + 几何体ID * 9];
		}
		for (uint8 i = 0; i < 3; ++i) {
			((float32*)&pos)[i] = mj->m_模拟数据->geom_xpos[i + 几何体ID * 3];
		}
		f_mat4_构建缩放矩阵(body_ptr[几何体ID]->m_变换.scale);

		S_物体* ob = (S_物体*)body_ptr[几何体ID]->m_绑定物体;
		rot = f_mat_3x3转置(rot);
		//Mat44f mat = f_mat44_构建(rot, pos) * f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
		Mat44f mat = f_mat44_构建(rot, pos);// *f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
		
		//body_ptr[几何体ID]->m_Matrix = f_mat4_构建移动矩阵(pos) * f_mat44_构建X旋转矩阵(-90) * f_mat44_构建Z旋转矩阵(0) * f_mat33_to_44(rot);
		//body_ptr[几何体ID]->m_Matrix = f_mat4_构建移动矩阵(pos);
		body_ptr[几何体ID]->m_Matrix = f_mat44_构建X旋转矩阵(-90) * (f_mat4_构建缩放矩阵({1,-1,1}) * mat);
		
	}
	

}

void f_PhyMujoco_添加物理体(S_PhysicsMujoco* 解算器, std::vector<S_物理体*>& body) {
	解算器->m_Body = body;


}

std::vector<S_物理体*> f_PhyMujoco_get几何图形(S_PhysicsMujoco* mj, E_物理碰撞几何类型 几何类型) {
	uint32 num = mj->m_Body.size();
	auto boyd_ptr = mj->m_Body.data();

	std::vector<S_物理体*> obs;
	obs.reserve(num);
	for (uint32 i = 0; i < num; ++i) {
		S_物理几何体* geom = ((S_物理几何体*)boyd_ptr[i]->m_Data);
		if (geom->m_Type == 几何类型) {
			obs.emplace_back(boyd_ptr[i]);
		}
	}
	return obs;
}

void f_PhyMujoco_构建内部几何体填充到物体(S_PhysicsMujoco* mj, std::vector<S_物理体*>& Body) {
	uint32 num = Body.size();
	auto boyd_ptr = Body.data();
	
	//for (uint32 i = 0; i < mj->m_模型->nbody; ++i) {
	for (uint32 i = 0; i < num; ++i) {
		S_物理体* body = boyd_ptr[i];
		if (!(body->m_绑定物体)) continue;

		//int32 几何体位置偏移 = mj->m_模型->body_geomadr[i];
		int32 几何体位置偏移 = boyd_ptr[i]->m_ID;

		//for (uint32 j = 0; j < mj->m_模型->body_geomnum[i]; ++j) {
		//int32 几何体位置偏移 = mj->m_模型->body_geomadr[i];
		//S_物理体* body = boyd_ptr[几何体位置偏移];
			
		switch (mj->m_模型->geom_type[几何体位置偏移]) {
		case mjtGeom::mjGEOM_PLANE:
			break;
		case mjtGeom::mjGEOM_SPHERE:
			break;
		case mjtGeom::mjGEOM_CAPSULE:break;
		case mjtGeom::mjGEOM_ELLIPSOID:break;
		case mjtGeom::mjGEOM_CYLINDER:break;
		case mjtGeom::mjGEOM_BOX:
			break;
		case mjtGeom::mjGEOM_MESH: {
			S_物体* 物体 = (S_物体*)body->m_绑定物体;
			
			int32 网格ID = mj->m_模型->geom_dataid[几何体位置偏移];

			uint32 vertOffset = mj->m_模型->mesh_vertadr[网格ID];
			uint32 IndexOffset = mj->m_模型->mesh_faceadr[网格ID];

			uint32 vertNum = mj->m_模型->mesh_vertnum[网格ID];
			uint32 faceNum = mj->m_模型->mesh_facenum[网格ID];

			std::vector<S_VNT1> vert;
			std::vector<uvec3> index;
			index.reserve(faceNum);
			for (uint32 k = 0; k < faceNum; ++k) {
				uvec3 face_index = ((uvec3*)mj->m_模型->mesh_face)[(k+IndexOffset)];

				S_VNT1 顶点0;
				顶点0.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.x + vertOffset];
				顶点0.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.x + vertOffset];
				顶点0.UV = {};
				S_VNT1 顶点1;
				顶点1.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.y + vertOffset];
				顶点1.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.y + vertOffset];
				顶点1.UV = {};
				S_VNT1 顶点2;
				顶点2.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.z + vertOffset];
				顶点2.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.z + vertOffset];
				顶点2.UV = {};

				vert.push_back(顶点0);
				vert.push_back(顶点1);
				vert.push_back(顶点2);
				index.push_back({k*3,k*3+1,k*3+2});
			}

			S_Mesh* me = f_ob_fromMesh(物体, DEF_网格主数据层);
			f_surface_fill顶点数据(me, vert);
			//f_surface_fill索引数据(me, index);
			f_surface_计算包围盒(me);
			//vec3 pos = ((vec3*)mj->m_模型->geom_pos)[几何体位置偏移];
			vec3 pos = ((vec3*)mj->m_模拟数据->geom_xpos)[几何体位置偏移];
			//vec3 pos = ((vec3*)mj->m_模型->body_pos)[i];
			int32 body链接ID = mj->m_模型->body_jntadr[几何体位置偏移];
			if (body链接ID >= 0) {
				pos = ((vec3*)mj->m_模型->jnt_pos)[body链接ID];
			}

			f_ob_setPos((S_物体*)body->m_绑定物体, pos);
			//for (uint32 k = 0; k < indexNum; ++k) {
			//	std::cout << "mj->m_模型->mesh_face[IndexOffset] = " << mj->m_模型->mesh_face[IndexOffset + k] << std::endl;
			//}
			break;
		}
		}
		//}
	}
}

void f_PhyMujoco_应用力(S_PhysicsMujoco* mj, const S_物理互交参数& 互交) {
	uint32 num = mj->m_Body.size();
	auto boyd_ptr = mj->m_Body.data();

	mjtNum rpoint[6];
	//rpoint[0] = 受力点[0].x;
	//rpoint[1] = 受力点[0].y;
	//rpoint[2] = 受力点[0].z;
	rpoint[3] = 1;
	rpoint[4] = 1;
	rpoint[5] = 1;
	mjtNum dir[3];
	//dir[0] = 受力点[1].x;
	//dir[1] = 受力点[1].y;
	//dir[2] = 受力点[1].z;

	//for (uint32 i = 0; i < num; ++i) {
		//S_物理体* body = boyd_ptr[i];
		//mj->m_模型->nv;
		//mj_objectAcceleration(mj->m_模型, mj->m_模拟数据, mj->m_模型->site_size[body->m_ID], body->m_ID, rpoint, 1);
	//}

	mjvPerturb p = {};
	p.active = true;
	p.select = 互交.bodyID;
	//p.refpos[0] = 互交.force.x;
	//p.refpos[1] = 互交.force.y;
	//p.refpos[2] = 互交.force.z;
	p.scale = 1;
	p.localpos[0] = 0;
	p.localpos[1] = 0;
	p.localpos[2] = 0;

	p.refpos[0] = 互交.vec.x;
	p.refpos[1] = 互交.vec.y;
	p.refpos[2] = 互交.vec.z;

	//p.refpos[0] = 0;
	//p.refpos[1] = 1;
	//p.refpos[2] = 0;

	p.localpos[0] = 互交.pos.x;
	p.localpos[1] = 互交.pos.y;
	p.localpos[2] = 互交.pos.z;
	

	mjtNum qfrc_target = 10;
	//mj_applyFT(mj->m_模型, mj->m_模拟数据, p.refpos, rpoint, rpoint, 0, &qfrc_target);
	//mjv_applyPerturbForce(mj->m_模型, mj->m_模拟数据, &p);
	mjv_applyPerturbPose(mj->m_模型, mj->m_模拟数据, &p, 0);
}

uint32 f_PhyMujoco_get肌腱数量(S_PhysicsMujoco* mj) {
	return mj->m_模型->ntendon;
}

void f_PhyMujoco_set肌腱力度(S_PhysicsMujoco* mj, float32* 力度, int32 i) {
	uint32 num = mj->m_模型->ntendon;

	//num = mj->m_模型->nu;
	/*for (uint32 i = 0; i < num; ++i) {
		auto a = mj->m_模型->actuator_trntype[i];
		auto b = mj->m_模型->actuator_dyntype[i];
		auto c = mj->m_模型->actuator_gaintype[i];
		auto d = mj->m_模型->actuator_trnid[i];
		auto e = mj->m_模型->actuator_lengthrange[i];
	}*/

	if (i >= 0 && i < num) {
		//int32 链接点数量 = mj->m_模拟数据->ten_wrapnum[i];
		//int32 链接点偏移 = mj->m_模拟数据->ten_wrapadr[i];
		//mj->m_模拟数据->ten_length[i] = 力度[0];
		//mj->m_模型->tendon_damping[i] = 力度[0];
		mj->m_模型->tendon_lengthspring[i] = 力度[0];
		//mj->m_模型->tendon_stiffness[i] = 力度[0];
		//mj->m_模型->tendon_invweight0[i] = 力度[0];
		//mj->m_模型->tendon_range[i] = 力度[0];
		//mj->m_模拟数据->ten_length[i] = 力度[0];
	}
	else {
		for (uint32 i = 0; i < num; ++i) {
			//mj->m_模拟数据->ten_length[i] = 力度[0];
			//mj->m_模型->tendon_damping[i] = 力度[0];
			mj->m_模型->tendon_lengthspring[i] = 力度[0];
			//mj->m_模型->tendon_stiffness[i] = 力度[0];
			//mj->m_模型->tendon_invweight0[i] = 力度[0];
			//mj->m_模型->tendon_range[i] = 力度[0];
			//mj->m_模拟数据->ten_length[i] = 力度[0];
		}
	}
}

void f_PhyMujoco_构建肌腱物体可视(S_PhysicsMujoco* mj, S_物体* Body) {

	int32 num = mj->m_模型->ntendon;

	auto mat = f_mat4_右手转左手坐标系(f_mat44_identity());

	for (uint32 i = 0; i < num; ++i) {
		uint32 tendon_num = mj->m_模型->tendon_num[i];

		int32 offset = mj->m_模型->tendon_adr[i];

		mjtObj obtype = (mjtObj)mj->m_模型->wrap_type[offset];
		int32 objid = mj->m_模型->wrap_objid[offset];


		S_Mesh* mesh = f_ob_Mesh(Body, i);
		

		int32 链接点数量 = mj->m_模拟数据->ten_wrapnum[i];
		int32 链接点偏移 = mj->m_模拟数据->ten_wrapadr[i];
		std::vector<S_VC> 控制点(链接点数量);
		for (int32 j = 0; j < 链接点数量; ++j) {
			int32 id = (j + 链接点偏移) * 3;
			S_VC point;
			point.vert.x = mj->m_模拟数据->wrap_xpos[id];
			point.vert.y = mj->m_模拟数据->wrap_xpos[id+1];
			point.vert.z = mj->m_模拟数据->wrap_xpos[id+2];
			point.vert = mat * point.vert;
			point.color = f_PackData4X8({ 200,60,10,255 });
			控制点[j] = point;
		}
		f_surface_fill顶点数据(mesh, 控制点);

	}

	

}




/*int bodyid = mj_name2id(m, mjOBJ_BODY, "myfloatingbody");
int qposadr = -1, qveladr = -1;

// make sure we have a floating body: it has a single free joint
if (bodyid >= 0 && m->body_jntnum[bodyid] == 1 &&
	m->jnt_type[m->body_jntadr[bodyid]] == mjJNT_FREE)
{
	// extract the addresses from the joint specification
	qposadr = m->jnt_qposadr[m->body_jntadr[bodyid]];
	qveladr = m->jnt_dofadr[m->body_jntadr[bodyid]];
}*/

